#!/usr/bin/env python3

import rospy
import re
import paho.mqtt.client as mqtt
from geometry_msgs.msg import Pose2D, PoseArray, Pose

class MqttSubscriber(object):
    def __init__(self):
        self.mqtt_client = mqtt.Client()
        self.mqtt_client.on_connect = self.on_connect
        self.mqtt_client.on_message = self.on_message
        self.mqtt_client.connect("test.mosquitto.org", 1883, 60)
        self.mqtt_client.loop_start()

        self.waypoints_publisher = rospy.Publisher('/waypoints', PoseArray, queue_size=10)

    def on_connect(self, client, userdata, flags, rc):
        rospy.logwarn("[waypoints_to_ros]Connected to MQTT broker with result code %s", str(rc))
        client.subscribe("wpr_waypoint_pos")

    # 从MQTT网络接收数据，并发布到ROS网络中
    def on_message(self, client, userdata, mqtt_msg):
        # rospy.loginfo("Received message on topic %s:  %s", mqtt_msg.topic, mqtt_msg.payload)
        if mqtt_msg.topic == "wpr_waypoint_pos":
            pattern = r'\((-?\d+\.\d+),(-?\d+\.\d+)\)'
            payload_str = mqtt_msg.payload.decode('utf-8')
            matches = re.findall(pattern, payload_str)
            pose_array = PoseArray()
            rospy.loginfo("[waypoints_to_ros] 接收到MQTT服务器发来 航点坐标:")
            for match in matches:
                lat, lng = match
                lat = float(lat)
                lng = float(lng)
                rospy.loginfo("( %f , %f )", lat, lng)
                pose = Pose()
                pose.position.x = lat
                pose.position.y = lng
                pose_array.poses.append(pose)
            self.waypoints_publisher.publish(pose_array)
            rospy.loginfo("--------------------")

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    rospy.init_node('waypoints_to_ros')
    mqtt_subscriber = MqttSubscriber()
    mqtt_subscriber.run()